
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mp_autotune.c
  * @author     baiyang
  * @date       2023-6-29
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mp_autotune.h"

#include <rtthread.h>

#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
#include <sensor_imu/sensor_imu.h>
#endif

#include <logger/blog.h>
#include <gcs_mavlink/gcs.h>
#include <common/time/gp_time.h>
/*-----------------------------------macro------------------------------------*/
// step size for changing FF gains, percentage
#define AUTOTUNE_INCREASE_FF_STEP 12
#define AUTOTUNE_DECREASE_FF_STEP 15

// limits on IMAX
#define AUTOTUNE_MIN_IMAX 0.4f
#define AUTOTUNE_MAX_IMAX 0.9f

// ratio of I to P
#define AUTOTUNE_I_RATIO 0.75f

// time constant of rate trim loop
#define TRIM_TCONST 1.0f

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
 # define Debug(fmt, ...)  do {rt_kprintf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ##__VA_ARGS__); } while(0)
#else
 # define Debug(fmt, ...)
#endif
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static struct ATGains _get_gains(mp_autotune_t autotune);
static void _update_rmax(mp_autotune_t autotune);
static void _save_gains(mp_autotune_t autotune);
static void _restore_gains(mp_autotune_t autotune);
static const char *_axis_string(const struct mp_autotune *autotune);
static void _state_change(mp_autotune_t autotune, enum ATState new_state);
/*----------------------------------variable----------------------------------*/
/*
  auto-tuning table. This table gives the starting values for key
  tuning parameters based on a user chosen AUTOTUNE_LEVEL parameter
  from 1 to 10. Level 1 is a very soft tune. Level 10 is a very
  aggressive tune.
  Level 0 means use the existing RMAX and TCONST parameters
 */
static const struct {
    float tau;
    float rmax;
} tuning_table[] = {
    { 1.00f, 20 },   // level 1
    { 0.90f, 30 },   // level 2
    { 0.80f, 40 },   // level 3
    { 0.70f, 50 },   // level 4
    { 0.60f, 60 },   // level 5
    { 0.50f, 75 },   // level 6
    { 0.30f, 90 },   // level 7
    { 0.2f, 120 },   // level 8
    { 0.15f, 160 },   // level 9
    { 0.1f, 210 },   // level 10
    { 0.1f, 300 },   // (yes, it goes to 11)
};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
mp_autotune_t mp_autotune_probe(struct ATGains *_gains, enum ATType type, mb_fixed_wing_const_t parms, PID_ctrl *rpid, float dt)
{
    mp_autotune_t autotune = (mp_autotune_t)rt_malloc(sizeof(struct mp_autotune));

    if (!autotune) {
        return NULL;
    }

    mp_autotune_ctor(autotune, _gains, type, parms, rpid, dt);

    return autotune;
}

// constructor
void mp_autotune_ctor(mp_autotune_t autotune, struct ATGains *_gains, enum ATType type, mb_fixed_wing_const_t parms, PID_ctrl *rpid, float dt)
{
    rt_memset(autotune, 0, sizeof(struct mp_autotune));

    autotune->current = _gains;
    autotune->rpid = rpid;
    autotune->type = type;
    autotune->aparm = parms;
    autotune->dt    = dt;

    mff5_ctor(&autotune->ff_filter, 2);

    slew_limiter_ctor(&autotune->slew_limiter_P, &autotune->slew_limit_max, &autotune->slew_limit_tau);
    slew_limiter_ctor(&autotune->slew_limiter_D, &autotune->slew_limit_max, &autotune->slew_limit_tau);
}

// called when autotune mode is entered
void mp_autotune_start(mp_autotune_t autotune)
{
    autotune->running = true;
    autotune->state = MP_AT_IDLE;

    autotune->restore = autotune->last_save = _get_gains(autotune);
    *autotune->current = autotune->restore;

    // do first update of rmax and tau now
    _update_rmax(autotune);

    autotune->rpid->_kimax = math_constrain_float(autotune->rpid->_kimax, AUTOTUNE_MIN_IMAX, AUTOTUNE_MAX_IMAX);

    // use 0.75Hz filters on the actuator, rate and target to reduce impact of noise
    lpf_set_cutoff2(&autotune->actuator_filter, 0.75f, autotune->dt);
    lpf_set_cutoff2(&autotune->rate_filter, 0.75f, autotune->dt);

    // target filter is a bit broader
    lpf_set_cutoff2(&autotune->target_filter, 4, autotune->dt);

    mff5_reset(&autotune->ff_filter);
    lpf_reset2(&autotune->actuator_filter);
    lpf_reset2(&autotune->rate_filter);
    autotune->D_limit = 0;
    autotune->P_limit = 0;
    autotune->ff_count = 0;
    autotune->D_set_ms = 0;
    autotune->P_set_ms = 0;
    autotune->done_count = 0;

    if (!math_flt_positive(pid_ctrl_get_slew_limit(autotune->rpid))) {
        // we must have a slew limit, default to 150 deg/s
        param_t *param = NULL;

        const char *param_group_names[] = { "RLL_CTRL", "PTCH_CTRL", "YAW_CTRL"};

        for (uint8_t i=0; i<ARRAY_SIZE(param_group_names);i++) {
            param = param_get_by_variable2(param_group_names[i], (void*)&autotune->rpid->_slew_rate_max);
            if (param != NULL) {
                param_set_and_save(param, 150);
                break;
            }
        }
    }

    if (autotune->current->FF < 0.01f) {
        // don't allow for zero FF
        autotune->current->FF = 0.01f;
        autotune->rpid->_kff = autotune->current->FF;
    }

    Debug("START FF -> %.3f\n", autotune->rpid->_kff);
}

// called to stop autotune and restore gains when user leaves
// autotune
void mp_autotune_stop(mp_autotune_t autotune)
{
    if (autotune->running) {
        autotune->running = false;
        if (math_flt_positive(autotune->D_limit) && math_flt_positive(autotune->P_limit)) {
            _save_gains(autotune);
        } else {
            _restore_gains(autotune);
        }
    }
}

// update called whenever autotune mode is active. This is
// called at the main loop rate
void mp_autotune_update(mp_autotune_t autotune, pid_info_t pid_info, float scaler, float angle_err_deg)
{
    if (!autotune->running) {
        return;
    }

    // see what state we are in
    enum ATState new_state = autotune->state;
    const float desired_rate = lpf_apply1(&autotune->target_filter, pid_info->target);

    // filter actuator without I term so we can take ratios without
    // accounting for trim offsets. We first need to include the I and
    // clip to 45 degrees to get the right value of the real surface
    const float clipped_actuator = math_constrain_float(pid_info->FF + pid_info->P + pid_info->D + pid_info->I, -45, 45) - pid_info->I;
    const float actuator = lpf_apply1(&autotune->actuator_filter, clipped_actuator);
    const float actual_rate = lpf_apply1(&autotune->rate_filter, pid_info->actual);

    autotune->max_actuator = MAX(autotune->max_actuator, actuator);
    autotune->min_actuator = MIN(autotune->min_actuator, actuator);
    autotune->max_rate = MAX(autotune->max_rate, actual_rate);
    autotune->min_rate = MIN(autotune->min_rate, actual_rate);
    autotune->max_target = MAX(autotune->max_target, desired_rate);
    autotune->min_target = MIN(autotune->min_target, desired_rate);
    autotune->max_P = MAX(autotune->max_P, fabsf(pid_info->P));
    autotune->max_D = MAX(autotune->max_D, fabsf(pid_info->D));
    autotune->min_Dmod = MIN(autotune->min_Dmod, pid_info->Dmod);
    autotune->max_Dmod = MAX(autotune->max_Dmod, pid_info->Dmod);

    // update the P and D slew rates, using P and D values from before Dmod was applied
    const float slew_limit_scale = 45.0f / degrees(1);
    autotune->slew_limit_max = autotune->rpid->_slew_rate_max;
    autotune->slew_limit_tau = 1.0f;
    slew_limiter_modifier(&autotune->slew_limiter_P, (pid_info->P/pid_info->Dmod)*slew_limit_scale, autotune->dt);
    slew_limiter_modifier(&autotune->slew_limiter_D, (pid_info->D/pid_info->Dmod)*slew_limit_scale, autotune->dt);

    // remember maximum slew rates for this cycle
    autotune->max_SRate_P = MAX(autotune->max_SRate_P, autotune->slew_limiter_P._output_slew_rate);
    autotune->max_SRate_D = MAX(autotune->max_SRate_D, autotune->slew_limiter_D._output_slew_rate);

    float att_limit_deg = 0;
    switch (autotune->type) {
    case MP_AUTOTUNE_ROLL:
        att_limit_deg = autotune->aparm->roll_limit_cd * 0.01f;
        break;
    case MP_AUTOTUNE_PITCH:
        att_limit_deg = MIN(abs(autotune->aparm->pitch_limit_max_cd),abs(autotune->aparm->pitch_limit_min_cd))*0.01f;
        break;
    case MP_AUTOTUNE_YAW:
        // arbitrary value for yaw angle
        att_limit_deg = 20;
        break;
    }

    // thresholds for when we consider an event to start and end
    const float rate_threshold1 = 0.4f * MIN(att_limit_deg / autotune->current->tau, autotune->current->rmax_pos);
    const float rate_threshold2 = 0.25f * rate_threshold1;
    bool in_att_demand = fabsf(angle_err_deg) >= 0.3f * att_limit_deg;

    switch (autotune->state) {
    case MP_AT_IDLE:
        if (desired_rate > rate_threshold1 && in_att_demand) {
            new_state = MP_AT_DEMAND_POS;
        } else if (desired_rate < -rate_threshold1 && in_att_demand) {
            new_state = MP_AT_DEMAND_NEG;
        }
        break;
    case MP_AT_DEMAND_POS:
        if (desired_rate < rate_threshold2) {
            new_state = MP_AT_IDLE;
        }
        break;
    case MP_AT_DEMAND_NEG:
        if (desired_rate > -rate_threshold2) {
            new_state = MP_AT_IDLE;
        }
        break;
    }

    const uint32_t now = time_millis();

    if (now - autotune->last_log_ms >= 40) {
        // log at 25Hz
        struct log_ATRP pkt = {
            APLOG_PACKET_HEADER_INIT(LOG_ATRP_MSG),
            .time_us = time_micros64(),
            .type = (uint8_t)(autotune->type),
            .state = (uint8_t)(new_state),
            .actuator = actuator,
            .P_slew = autotune->max_SRate_P,
            .D_slew = autotune->max_SRate_D,
            .FF_single = autotune->FF_single,
            .FF = autotune->current->FF,
            .P = autotune->current->P,
            .I = autotune->current->I,
            .D = autotune->current->D,
            .action = (uint8_t)(autotune->action),
            .rmax = (float)(autotune->current->rmax_pos),
            .tau = autotune->current->tau
        };
        blog_write_critical_block(&pkt, sizeof(pkt));
        autotune->last_log_ms = now;
    }

    if (new_state == autotune->state) {
        if (autotune->state == MP_AT_IDLE &&
            now - autotune->state_enter_ms > 500 &&
            autotune->max_Dmod < 0.9f) {
            // we've been oscillating while idle, reduce P or D
            const float slew_sum = autotune->max_SRate_P + autotune->max_SRate_D;
            const float gain_mul = 0.5f;
            autotune->current->P *= math_linear_interpolate(gain_mul, 1.0f,
                                            autotune->max_SRate_P,
                                            slew_sum, 0);
            autotune->current->D *= math_linear_interpolate(gain_mul, 1.0f,
                                            autotune->max_SRate_D,
                                            slew_sum, 0);
            autotune->rpid->_kp = autotune->current->P;
            autotune->rpid->_kd = autotune->current->D;
            autotune->action = MP_AT_IDLE_LOWER_PD;
            autotune->P_limit = MIN(autotune->P_limit, autotune->current->P);
            autotune->D_limit = MIN(autotune->D_limit, autotune->current->D);
            _state_change(autotune, autotune->state);
        }
        return;
    }

    if (new_state != MP_AT_IDLE) {
        // starting an event
        autotune->min_actuator = autotune->max_actuator = autotune->min_rate = autotune->max_rate = 0;
        autotune->state_enter_ms = now;
        autotune->state = new_state;
        return;
    }

    if ((autotune->state == MP_AT_DEMAND_POS && autotune->max_rate < 0.01f * autotune->current->rmax_pos) ||
        (autotune->state == MP_AT_DEMAND_NEG && autotune->min_rate > -0.01f * autotune->current->rmax_neg)) {
        // we didn't get enough rate
        autotune->action = MP_AT_LOW_RATE;
        _state_change(autotune, MP_AT_IDLE);
        return;
    }

    if (now - autotune->state_enter_ms < 100) {
        // not long enough sample
        autotune->action = MP_AT_SHORT;
        _state_change(autotune, MP_AT_IDLE);
        return;
    }

    // we've finished an event. calculate the single-event FF value
    if (autotune->state == MP_AT_DEMAND_POS) {
        autotune->FF_single = autotune->max_actuator / (autotune->max_rate * scaler);
    } else {
        autotune->FF_single = autotune->min_actuator / (autotune->min_rate * scaler);
    }

    // apply median filter
    float FF = mff5_apply(&autotune->ff_filter, autotune->FF_single);
    autotune->ff_count++;

    const float old_FF = autotune->rpid->_kff;

    // limit size of change in FF
    FF = math_constrain_float(FF,
                         old_FF*(1-AUTOTUNE_DECREASE_FF_STEP*0.01f),
                         old_FF*(1+AUTOTUNE_INCREASE_FF_STEP*0.01f));

    // adjust P and D
    float D = autotune->rpid->_kd;
    float P = autotune->rpid->_kp;

    if (autotune->ff_count == 1) {
        // apply minimum D and P values
        D = MAX(D, 0.0005f);
        P = MAX(P, 0.01f);
    } else if (autotune->ff_count == 4) {
        // we got a good ff estimate, halve P ready to start raising D
        P *= 0.5f;
    }

    // see if the slew limiter kicked in
    if (autotune->min_Dmod < 1.0f && !math_flt_positive(autotune->D_limit)) {
        // oscillation, without D_limit set
        if (autotune->max_P > 0.5f * autotune->max_D) {
            // lower P and D to get us to a non-oscillating state
            P *= 0.35f;
            D *= 0.75f;
            autotune->action = MP_AT_LOWER_PD;
        } else {
            // set D limit to 30% of current D, remember D limit and start to work on P
            D *= 0.3f;
            autotune->D_limit = D;
            autotune->D_set_ms = now;
            autotune->action = MP_AT_LOWER_D;
            GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%sD: %.4f", _axis_string(autotune), autotune->D_limit);
        }
    } else if (autotune->min_Dmod < 1.0f) {
        // oscillation, with D_limit set
        if (now - autotune->D_set_ms > 2000) {
            // leave 2s for Dmod to settle after lowering D
            if (autotune->max_D > 0.8f * autotune->max_P) {
                // lower D limit some more
                D *= 0.35f;
                autotune->D_limit = D;
                autotune->D_set_ms = now;
                autotune->action = MP_AT_LOWER_D;
                GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%sD: %.4f", _axis_string(autotune), autotune->D_limit);
                autotune->done_count = 0;
            } else if (now - autotune->P_set_ms > 2500) {
                if (math_flt_positive(autotune->P_limit)) {
                    // if we've already got a P estimate then don't
                    // reduce as quickly, stopping small spikes at the
                    // later part of the tune from giving us a very
                    // low P gain
                    P *= 0.7f;
                } else {
                    P *= 0.35f;
                }
                autotune->P_limit = P;
                autotune->P_set_ms = now;
                autotune->action = MP_AT_LOWER_P;
                autotune->done_count = 0;
                GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%sP: %.4f", _axis_string(autotune), autotune->P_limit);
            }
        }
    } else if (autotune->ff_count < 4) {
        // we don't have a good FF estimate yet, keep going

    } else if (!math_flt_positive(autotune->D_limit)) {
        /* we haven't detected D oscillation yet, keep raising D */
        D *= 1.3f;
        autotune->action = MP_AT_RAISE_D;
    } else if (!math_flt_positive(autotune->P_limit)) {
        /* not oscillating, increase P gain */
        P *= 1.3;
        autotune->action = MP_AT_RAISE_PD;
    } else {
        // after getting P_limit we consider the tune done when we
        // have done 3 cycles without reducing P
        if (autotune->done_count < 3) {
            if (++autotune->done_count == 3) {
                GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s: Finished", _axis_string(autotune));
                _save_gains(autotune);
            }
        }
    }

    autotune->rpid->_kff = FF;
    autotune->rpid->_kp  = P;
    autotune->rpid->_kd  = D;
    if (autotune->type == MP_AUTOTUNE_ROLL) {  // for roll set I = smaller of FF or P
        autotune->rpid->_ki = MIN(P, (FF / TRIM_TCONST));
    } else {                      // for pitch/yaw naturally damped axes) set I usually = FF to get 1 sec I closure
        autotune->rpid->_ki = MAX(P*AUTOTUNE_I_RATIO, (FF / TRIM_TCONST));
    }

    // setup filters to be suitable for time constant and gyro filter
    autotune->rpid->_filt_t_hz = (10.0/(autotune->current->tau * 2 * M_PI));
    autotune->rpid->_filt_e_hz = (0);
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
    autotune->rpid->_filt_d_hz = (sensor_imu_get_singleton()->_gyro_filter_cutoff * 0.5f);
#else
    autotune->rpid->_filt_d_hz = (20 * 0.5f);
#endif

    autotune->current->FF = FF;
    autotune->current->P = P;
    autotune->current->I = autotune->rpid->_ki;
    autotune->current->D = D;

    Debug("FPID=(%.3f, %.3f, %.3f, %.3f) Dmod=%.2f\n",
          autotune->rpid->_kff,
          autotune->rpid->_kp,
          autotune->rpid->_ki,
          autotune->rpid->_kd,
          autotune->min_Dmod);

    // move rmax and tau towards target
    _update_rmax(autotune);

    _state_change(autotune, new_state);
}

/*
  get gains with PID components
 */
static struct ATGains _get_gains(mp_autotune_t autotune)
{
    struct ATGains ret = *(autotune->current);
    ret.FF = autotune->rpid->_kff;
    ret.P = autotune->rpid->_kp;
    ret.I = autotune->rpid->_ki;
    ret.D = autotune->rpid->_kd;
    ret.IMAX = autotune->rpid->_kimax;
    ret.flt_T = autotune->rpid->_filt_t_hz;
    ret.flt_E = autotune->rpid->_filt_e_hz;
    ret.flt_D = autotune->rpid->_filt_d_hz;
    return ret;
}

/*
  update RMAX and TAU parameters on each step. We move them gradually
  towards the target to allow for a user going straight to a level 10
  tune while starting with a poorly tuned plane
 */
static void _update_rmax(mp_autotune_t autotune)
{
    uint8_t level = math_constrain_int32(autotune->aparm->autotune_level, 0, ARRAY_SIZE(tuning_table));

    int16_t target_rmax;
    float target_tau;

    if (level == 0) {
        // this level means to keep current values of RMAX and TCONST
        target_rmax = math_constrain_float(autotune->current->rmax_pos, 75, 720);
        target_tau = math_constrain_float(autotune->current->tau, 0.1f, 2);
    } else {
        target_rmax = tuning_table[level-1].rmax;
        target_tau = tuning_table[level-1].tau;
        if (autotune->type == MP_AUTOTUNE_PITCH) {
            // 50% longer time constant on pitch
            target_tau *= 1.5f;
        }
    }

    if (level > 0 && math_flt_positive(autotune->current->FF)) {
        const float invtau = ((1.0f / target_tau) + (autotune->current->I / autotune->current->FF));
        if (math_flt_positive(invtau)) {
            target_tau = MAX(target_tau,1.0f / invtau);
        }
    }

    if (autotune->current->rmax_pos == 0) {
        // conservative initial value
        autotune->current->rmax_pos = 75;
    }
    // move RMAX by 20 deg/s per step
    autotune->current->rmax_pos = math_constrain_int32(target_rmax,
                                         autotune->current->rmax_pos-20,
                                         autotune->current->rmax_pos+20);

    if (level != 0 || autotune->current->rmax_neg == 0) {
        autotune->current->rmax_neg = autotune->current->rmax_pos;
    }

    // move tau by max 15% per loop
    autotune->current->tau = math_constrain_float(target_tau,
                                    autotune->current->tau*0.85,
                                    autotune->current->tau*1.15);
}

/*
  save a set of gains
 */
static void _save_gains(mp_autotune_t autotune)
{
    const struct ATGains *v = &autotune->last_save;

    param_t *param = NULL;
    const char *param_group_names[] = { "RLL_CTRL", "PTCH_CTRL", "YAW_CTRL"};

    for (uint8_t i=0; i<ARRAY_SIZE(param_group_names);i++) {
        param = param_get_by_variable2(param_group_names[i], (void*)&autotune->current->tau);
        if (param == NULL) {
            continue;
        }

        if (param != NULL && !math_flt_equal(autotune->current->tau, v->tau)) {
            param_set_and_save(param, autotune->current->tau);
        }

        param = param_get_by_variable2(param_group_names[i], (void*)&autotune->current->rmax_pos);
        if (param != NULL && (autotune->current->rmax_pos != v->rmax_pos)) {
            param_set_and_save(param, autotune->current->rmax_pos);
        }

        param = param_get_by_variable2(param_group_names[i], (void*)&autotune->current->rmax_neg);
        if (param != NULL && (autotune->current->rmax_neg != v->rmax_neg)) {
            param_set_and_save(param, autotune->current->rmax_neg);
        }

        param = param_get_by_variable2(param_group_names[i], (void*)&autotune->rpid->_kff);
        if (param != NULL && !math_flt_equal(autotune->rpid->_kff, v->FF)) {
            param_set_and_save(param, autotune->rpid->_kff);
        }

        param = param_get_by_variable2(param_group_names[i], (void*)&autotune->rpid->_kp);
        if (param != NULL && !math_flt_equal(autotune->rpid->_kp, v->P)) {
            param_set_and_save(param, autotune->rpid->_kp);
        }

        param = param_get_by_variable2(param_group_names[i], (void*)&autotune->rpid->_ki);
        if (param != NULL && !math_flt_equal(autotune->rpid->_ki, v->I)) {
            param_set_and_save(param, autotune->rpid->_ki);
        }

        param = param_get_by_variable2(param_group_names[i], (void*)&autotune->rpid->_kd);
        if (param != NULL && !math_flt_equal(autotune->rpid->_kd, v->D)) {
            param_set_and_save(param, autotune->rpid->_kd);
        }

        param = param_get_by_variable2(param_group_names[i], (void*)&autotune->rpid->_kimax);
        if (param != NULL && !math_flt_equal(autotune->rpid->_kimax, v->IMAX)) {
            param_set_and_save(param, autotune->rpid->_kimax);
        }

        param = param_get_by_variable2(param_group_names[i], (void*)&autotune->rpid->_filt_t_hz);
        if (param != NULL && !math_flt_equal(autotune->rpid->_filt_t_hz, v->flt_T)) {
            param_set_and_save(param, autotune->rpid->_filt_t_hz);
        }

        param = param_get_by_variable2(param_group_names[i], (void*)&autotune->rpid->_filt_e_hz);
        if (param != NULL && !math_flt_equal(autotune->rpid->_filt_e_hz, v->flt_E)) {
            param_set_and_save(param, autotune->rpid->_filt_e_hz);
        }

        param = param_get_by_variable2(param_group_names[i], (void*)&autotune->rpid->_filt_d_hz);
        if (param != NULL && !math_flt_equal(autotune->rpid->_filt_d_hz, v->flt_D)) {
            param_set_and_save(param, autotune->rpid->_filt_d_hz);
        }
    }

    autotune->last_save = _get_gains(autotune);
}

/*
  set gains with PID components
 */
static void _restore_gains(mp_autotune_t autotune)
{
    *autotune->current = autotune->restore;

    autotune->rpid->_kff = autotune->restore.FF;
    autotune->rpid->_kp  = autotune->restore.P;
    autotune->rpid->_ki  = autotune->restore.I;
    autotune->rpid->_kd  = autotune->restore.D;
    autotune->rpid->_kimax = autotune->restore.IMAX;
    autotune->rpid->_filt_t_hz = autotune->restore.flt_T;
    autotune->rpid->_filt_e_hz = autotune->restore.flt_E;
    autotune->rpid->_filt_d_hz = autotune->restore.flt_D;
}

static const char *_axis_string(const struct mp_autotune *autotune)
{
    switch (autotune->type) {
    case MP_AUTOTUNE_ROLL:
        return "Roll";
    case MP_AUTOTUNE_PITCH:
        return "Pitch";
    case MP_AUTOTUNE_YAW:
        return "Yaw";
    }
    return "";
}

/*
  record a state change
 */
static void _state_change(mp_autotune_t autotune, enum ATState new_state)
{
    autotune->min_Dmod = 1;
    autotune->max_Dmod = 0;
    autotune->max_SRate_P = 1;
    autotune->max_SRate_D = 1;
    autotune->max_P = autotune->max_D = 0;
    autotune->state = new_state;
    autotune->state_enter_ms = time_millis();
}
/*------------------------------------test------------------------------------*/


